#!/usr/bin/python
# -*- coding: utf-8 -*-

import os
import Ice

try:
    HOME = os.environ['ROBOTS']
except KeyError:
    print 'ROBOTS environment variable is not currently defined.'
    sys.exit(0)

SLICE = os.path.join(HOME, 'slice')
FILE = os.path.join(SLICE, 'Robots.ice')
Ice.loadSlice('--all -I/usr/share/slice', ['-I' + SLICE, FILE])

import Robots
import math

def cartesianDist (a, b):
    return math.sqrt(math.pow(a.x - b.x, 2) + math.pow(a.y - b.y, 2))

def mobility (currPos, goalPos, nAgents, positions, mobility, fuzzy):

    for i in range (0, nAgents):
        angle = math.radians((360.0 / nAgents) * i)
        x = int(goalPos.pos.x + math.cos(angle) * goalPos.ratio)
        y = int(goalPos.pos.y + math.sin(angle) * goalPos.ratio)
        positions.append(Robots.Vec2(x, y))

        myDist = cartesianDist(currPos, positions[i])
        InputData = []
        InputData.append((('Distance', myDist), ('Autonomy', 4)))
        mobility.append(fuzzy.out(InputData[0])['Mobility'])

def closestPointToSpreadObject (mobility):

    key = -1
    mob = -10000

    for k, v in mobility.iteritems():
        if v > mob:
            mob = v
            key = k

    return key

def getDictKeyFromValue (d, value):

    for k, v in d.iteritems():
        if v.x == value.x and v.y == value.y:
            return k

    return None

def removeValueFromDict (d, value):

    key = -1
    for k, v in d.iteritems():
        if v.x == value.x and v.y == value.y:
            key = k
            
    return key

def proxyToString (prx):
    return str(prx.ice_getIdentity().name)

class InfoGoal:

    def __init__ (self, pos, mob):        
        self.pos = pos
        self.mob = mob

    def __str__ (self):        
        aux = 'Pos: [' + str(self.pos.x) + ', ' + str(self.pos.y) + '] Mob: ' + str(self.mob)
        return aux

class InfoGoals:

    def __init__ (self):
        self.goals = []

    def getBestPossibleGoal (self):

        mob = -10000
        goal = None

        for g in self.goals:
            if mob < g.mob:
                mob = g.mob
                goal = g

        return goal
